/*
 * NxPSDetectObstaclesBlobfinder.java
 *
 * Created on 17 de febrero de 2008, 08:27 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Behaviors;

import Robot.*;
import Robot.Sensors.Sensor;
import Robot.Sensors.Blobfinder;
import Utils.Vector2d;
import Utils.Pose2d;
/**
 *
 * @author alejo
 */
public class NxPSDetectObstaclesBlobfinder extends NxPSDetectObstacles {

    private Blobfinder bf;
    private float radius;
    private Position2d myPos;
    
    //TODO: Corregir, no se le deberia pasar el radio. Los vectores devuletos por el blobfinder ya deben tener en cuenta
    //el tamaño del objeto, es decir, no estar referenciados respecto a su centro, sino respecto al punto mas cercano en 
    //la dirección del robot
    public NxPSDetectObstaclesBlobfinder(String name,Position2d myPos,Blobfinder bf, float radius) {
        super(name);
        this.bf = bf;
        this.radius = radius ;
        this.myPos = myPos;


        
    }


    
    public Vector2d[] Value() {
        
        //extract  the real position of obstacles, taking the nearest point of the obstacle to the position of robot
        Vector2d[] obst = bf.read(Sensor.READ_MODE_NO_WAIT);
        Vector2d[] value;
        
        
            value = new Vector2d[obst.length];
            for(int i= 0;i<obst.length;i++)
            {
                
                //calculate vector from obstacle to position of robot
                Vector2d posVector = myPos.getPosition();
                if(!posVector.equals(null))
                    value[i] = posVector.sub(obst[i]);
                //define size of object
                value[i].setMag(radius);
                //define final position of object
                value[i] = value[i].add(obst[i]);
                /*
                System.out.print("x");
                System.out.print(i);
                System.out.print(": ");

                System.out.print(value[i].getX());
                System.out.print(" y");
                System.out.print(i);
                System.out.print(": ");

                System.out.println(value[i].getY());
                */

            }
       
            
        return value;
    }
}